#include "ArmMover.h"

ArmMover::ArmMover():m_loop_rate(10000){

	
	//Setting up the publishers
	m_arm_upper_elbow_pub = m_nodeHandler.advertise<std_msgs::Float64>("/elbow2_controller/command", 1000);
	m_arm_lower_elbow_pub = m_nodeHandler.advertise<std_msgs::Float64>("/elbow1_controller/command", 1000);
	m_arm_shoulder_pub = m_nodeHandler.advertise<std_msgs::Float64>("/shoulder_controller/command", 1000);
	m_arm_wrist_pub = m_nodeHandler.advertise<std_msgs::Float64>("/wrist_controller/command", 1000);
	m_arm_base_rotation_pub = m_nodeHandler.advertise<std_msgs::Float64>("/base_rotation_controller/command", 1000);
	m_arm_left_finger_pub = m_nodeHandler.advertise<std_msgs::Float64>("/left_finger_controller/command", 1000);
	m_arm_right_finger_pub = m_nodeHandler.advertise<std_msgs::Float64>("/right_finger_controller/command", 1000);
}


void ArmMover::DoCheck() {

	MoveUpperElbow(0);
	MoveLowerElbow(0);
	MoveShoulder(0);
	MoveWrist(0);
	RotateBase(0);
	MoveLeftFinger(0);
	MoveRightFinger(0);

	sleep(2);

	MoveUpperElbow(0);
	MoveLowerElbow(0);
	MoveShoulder(0);
	MoveWrist(0);
	RotateBase(0);
	MoveLeftFinger(0);
	MoveRightFinger(0);

	sleep(2);

	MoveUpperElbow(1);
	MoveLowerElbow(1);
	MoveShoulder(1);
	MoveWrist(1);
	RotateBase(1);
	MoveLeftFinger(1);
	MoveRightFinger(1);

	sleep(2);

	MoveUpperElbow(0.5);
	MoveLowerElbow(0.5);
	MoveShoulder(0.5);
	MoveWrist(0.5);
	RotateBase(-1);
	MoveLeftFinger(0.5);
	MoveRightFinger(0.5);
	

}


void ArmMover::MoveUpperElbow(double value){

	m_arm_upper_elbow_msg.data = value;
	m_arm_upper_elbow_pub.publish(m_arm_upper_elbow_msg);
	m_loop_rate.sleep();
	m_arm_upper_elbow_pub.publish(m_arm_upper_elbow_msg);	
}

void ArmMover::MoveLowerElbow(double value){

	m_arm_lower_elbow_msg.data = value;
	m_arm_lower_elbow_pub.publish(m_arm_lower_elbow_msg);
	m_loop_rate.sleep();
	m_arm_lower_elbow_pub.publish(m_arm_lower_elbow_msg);
}

void ArmMover::MoveShoulder(double value){

	m_arm_shoulder_msg.data = value;
	m_arm_shoulder_pub.publish(m_arm_shoulder_msg);
	m_loop_rate.sleep();
	m_arm_shoulder_pub.publish(m_arm_shoulder_msg);
}

void ArmMover::MoveWrist(double value){

	m_arm_wrist_msg.data = value;
	m_arm_wrist_pub.publish(m_arm_wrist_msg);
	m_loop_rate.sleep();
	m_arm_wrist_pub.publish(m_arm_wrist_msg);
}

//OR ROTATE??
void ArmMover::RotateBase(double value){

	m_arm_base_rotation_msg.data = value;
	m_arm_base_rotation_pub.publish(m_arm_base_rotation_msg);
	m_loop_rate.sleep();
	m_arm_base_rotation_pub.publish(m_arm_base_rotation_msg);
}

void ArmMover::MoveLeftFinger(double value){

	m_arm_left_finger_msg.data = value;
	m_arm_left_finger_pub.publish(m_arm_left_finger_msg);
	m_loop_rate.sleep();
	m_arm_left_finger_pub.publish(m_arm_left_finger_msg);
}

void ArmMover::MoveRightFinger(double value){

	m_arm_right_finger_msg.data = value;
	m_arm_right_finger_pub.publish(m_arm_right_finger_msg);
	m_loop_rate.sleep();
	m_arm_right_finger_pub.publish(m_arm_right_finger_msg);
}


void ArmMover::MoveShoulderAndUpperElbow(double shoulder_value,double upper_elbow_value){

	MoveShoulder(shoulder_value);
	MoveUpperElbow(upper_elbow_value);
}
